//
// Created by fazhehy on 2024/7/5.
//

#include "bldc_speed.h"

incremental_pid_t  bldc_speed_pid;

void bldc_speed_init()
{
    incremental_pid_init(&bldc_speed_pid,0.01f,0.02f,0,2000);
    timer_init(TC_TIME2_CH0, TIMER_US);
    timer_start(TC_TIME2_CH0);
}

uint32_t bldc_speed_get_value()
{
    uint32_t count = timer_get(TC_TIME2_CH0);
    timer_clear(TC_TIME2_CH0);
    return count;
}

void speed_control_callback()
{
//    if (bldc_target_speed != 0)
//        bldc_duty = (int16_t)incremental_pid(&bldc_speed_pid,(float )((int16_t )((bldc_speed)/100)), bldc_target_speed);
//    else
//        bldc_duty = 0;
    //    vofa_send_data(bldc_speed);
//    vofa_send();
}

float_t w_now=0,a=0,w_last=0;
float_t bldc_angle,bldc_angle_ref,bldc_angle_use,bldc_angle_add;
void bldc_angle_callback()
{
    w_now=bldc_speed+a;
    a=w_now-w_last;
    w_last=w_now;

    bldc_angle_ref=w_now*0.00032f;

    bldc_angle_use=bldc_angle+bldc_angle_add;

    if(bldc_angle_add>=59 || bldc_angle_add<=-59)
        bldc_angle_ref=0;
//    else if(bldc_angle_add<=-59)
//        bldc_angle_ref=0;
    bldc_angle_add+=bldc_angle_ref;
    vofa_send_data(bldc_angle_use);
    vofa_send();
}


void foc_callback()
{
    bldc_angle_use+=bldc_angle_ref/8;
//    bldc_foc_control(1.0f,0,(bldc_angle_use)*PI/180);//
}

void angle_calibration(uint8_t hall_value,int8_t turn_direction)//1-546231  -1-513264
{
    if(turn_direction==1)
    {
        switch (hall_value) {
            case 1:
                bldc_angle=120;
                break;
            case 3:
                bldc_angle=180;
                break;
            case 2:
                bldc_angle=240;
                break;
            case 6:
                bldc_angle=300;
                break;
            case 4:
                bldc_angle=0;
                break;
            case 5:
                bldc_angle=60;
                break;
            default:
                break;
        }
    }
    else
    {
        switch (hall_value) {
            case 1:
                bldc_angle=60;
                break;
            case 3:
                bldc_angle=120;
                break;
            case 2:
                bldc_angle=180;
                break;
            case 6:
                bldc_angle=240;
                break;
            case 4:
                bldc_angle=300;
                break;
            case 5:
                bldc_angle=360;
                break;
            default:
                break;
        }
    }
    bldc_angle_add=0;
}
